% directly from human data
% calculate 3D outputs 

addpath('./Func/')
addpath('./HumanData')
% name list 9 person
name  = {'Charles','Fred','Martin','Ram','Selina',... % 1st experiment
    'lily','po','ryan','vic'};  % 2nd experiment

% CONSTANT
% the number of subjects from the 1st experiment
exp1st = 5;
% the index of subjects use the right leg as swing leg during the step
rIndex = [6,7,9];
% experimental sampling frequency
fs = 480;
% % model = 'NAO';
model = 'HM';
% model  = 'AMBER';
time_rec = zeros(9,1);
LT = 0; % the length of the torso
whip = 0; % the width of hip
%
for index = 1:length(name) % 5

    % get human data, with breaking pts for each step
    humanData = getData(name{1,index});
   
    %     sum_weight  = sum_weight+humanData.weight; % for average weight
    % breaking pts for each step
    StartPt = humanData.sns_brk(1);
    EndPt = humanData.sns_brk(4);
    BrkPt = humanData.sns_brk(2);
%     humanData.sns_brk(3) = humanData.sns_brk(4);
%     ret = shift_data(BrkPt,EndPt,StartPt ,data);
    
    % position
    if index <= exp1st
        [xpos,ypos,zpos] = getPosCartesian(humanData); % subjects from experiment 1st
    else
        [xpos,ypos,zpos] = getPosCartesianNew3D(humanData); % subjects from experiment 2st
    end
       
        % calculate the length of torso
    Torso = calDist(xpos.hip_avg,ypos.hip_avg,zpos.hip_avg,...
        xpos.belly,ypos.belly,zpos.belly);
    LT = LT+Torso;
    HipWidth = calDist(xpos.lhip,ypos.lhip,zpos.lhip,...
        xpos.rhip,ypos.rhip,zpos.rhip);
    whip = whip+HipWidth;
    
    % 
    hip_disp = (HipWidth - humanData.hipjoint)/2;
    % z direction
            zpos.lhip = zpos.lhip - hip_disp;
            zpos.rhip = zpos.rhip + hip_disp;
    %%%%%%%%%%%  time  %%%%%%%%%%%%%%
    % scale time
    % from [c:d]
    ns_time_pre = (0:length(StartPt:EndPt)-1)'/fs;
    ns_ret = ns_time_pre;
    xpara_time = 1/ns_ret(end); % simply sacled time to 1
    ns_time = ns_ret*xpara_time;
    %%%% record time %%%%%%%%
    time_rec(index) = ns_ret(end);
    
    
    
    %%%%%%%%%%%% calculate human joint angles %%%%%%%%%%%%%%%%%
    if ismember(index,rIndex)
        
        angle = TwoStep3D_angle(xpos,ypos,zpos,humanData.sns_brk,2);
        
%         COM = cal_CoM3D(xpos,ypos,zpos,humanData,0);
%         slope = Knee3D_slope(xpos,ypos,zpos,humanData.sns_brk,2);
    else
        
        angle = TwoStep3D_angle(xpos,ypos,zpos,humanData.sns_brk,1);       
%         COM = cal_CoM3D(xpos,ypos,zpos,humanData,1);
%         slope = Knee3D_slope(xpos,ypos,zpos,humanData.sns_brk,1);        
    end
    nship_shift = shift_data(BrkPt-StartPt,size(angle.nship,2),1,angle.nship');
    ship_shift = shift_data(BrkPt-StartPt,size(angle.ship,2),1,angle.rhip');
%     figure(index)
%     plot(nship_shift,'b'); hold on;
%     plot(angle.nship,'g'); 
%     plot(angle.ship); hold on
%     plot(ship_shift,'m');hold on
    rhip_shift = shift_data(BrkPt-StartPt,size(angle.rhip,2),1,angle.rhip');
    plot(rhip_shift,'b'); hold on;
    lhip_shift = shift_data(BrkPt-StartPt,size(angle.lhip,2),1,angle.lhip');
    plot(lhip_shift,'r'); hold on;
    %%%%%%%%%%%% calculate human slope %%%%%%%%%%
%     if ismember(index,rIndex)
%         slope = Knee3D_slope(xpos,ypos,zpos,humanData.sns_brk,2);
%     else
%         slope = Knee3D_slope(xpos,ypos,zpos,humanData.sns_brk,1);
%     end
    % 1. left hip roll angle
    [ave_datalhip,ave_timelhip] = BeamDataTwoSteps(ns_time,lhip_shift');
    ave_datalhip_s(index,:) = ave_datalhip;
    % 2. right hip roll angle
    [ave_datarhip,ave_timerhip] = BeamDataTwoSteps(ns_time,rhip_shift');
    ave_datarhip_s(index,:) = ave_datarhip;

% %     % 1. stance ankle roll angle
% %     [ave_dataSA,ave_timeSA] = BeamData(ns_time,angle.sa);
% %     ave_dataSA_s(index,:) = ave_dataSA;
% %     
% %     % 2. stance hip roll angle
% %     [ave_dataShip,ave_timeShip] = BeamData(ns_time,angle.ship);
% %     ave_dataShip_s(index,:) = ave_dataShip;
% % 
% %     % 3. non-stance hip roll angle
% %     [ave_dataNShip,ave_timeNShip] = BeamData(ns_time,angle.nship);
% %     ave_dataNShip_s(index,:) = ave_dataNShip;
% %     
% %     
% %     % 4. torso roll angle
% %     [ave_dataTorso,ave_timeTorso] = BeamData(ns_time,angle.torso);
% %     ave_dataTorso_s(index,:) = ave_dataTorso;
% %     
% %     % 5. hip y position
% %     [ave_dataHipy,ave_timeHipy] = BeamData(ns_time,slope.hipy);
% %     ave_dataHipy_s(index,:) = ave_dataHipy;
% %     
% %     % 6. stance hip y position
% %     [ave_dataSHipy,ave_timeSHipy] = BeamData(ns_time,slope.shipy);
% %     ave_dataSHipy_s(index,:) = ave_dataSHipy;
% %     
% %     % 7. non-stance hip y position
% %     [ave_dataNSHipy,ave_timeSHipy] = BeamData(ns_time,slope.nshipy);
% %     ave_dataNSHipy_s(index,:) = ave_dataNSHipy;
% %     
% %     % 8. non-stance ankle y position
% %     [ave_dataNSAy,ave_timeNSAy] = BeamData(ns_time,slope.nsay);
% %     ave_dataNSAy_s(index,:) = ave_dataNSAy;
end
%
ave_time = ave_timerhip;
LT = LT/9;
whip = whip/9;
% cal;culate the mean value and save
%%%%%%%%%%%% angles %%%%%%%%%%%%%%%%%%%%%%%%%
% % 1. left hip angle
[x_meanlhip,upperBlhip,lowerBlhip]=meanValue(ave_datalhip_s);
% % 2. right hip angle
[x_meanrhip,upperBrhip,lowerBrhip]=meanValue(ave_datarhip_s);

% OptFit2(ave_time,upperBTH,lowerBTH,x_meanTH,'Torso','Angle(rad)',...
%     a0(5,:),2,ns_time_mean,'$\theta_{torso}^H$','$\theta_{torso}^d$',[-0.2 0.4])

% % 1. stance ankle angle
% [x_meanSA,upperBSA,lowerBSA]=meanValue(ave_dataSA_s);
% % 2. stance hip angle
% [x_meanShip,upperBShip,lowerBShip]=meanValue(ave_dataShip_s);
% % 3. non-stance hip angle
% [x_meanNShip,upperBNShip,lowerBNShip]=meanValue(ave_dataNShip_s);
% % 4. torso angle
% [x_meanTorso,upperBTorso,lowerBTorso]=meanValue(ave_dataTorso_s);
% 
% % 5. hip position on z direction
% [x_meanHipy,upperBHipy,lowerBHipy]=meanValue(ave_dataHipy_s);
% 
% % 6. ship position on y direction
% [x_meanSHipy,upperBSHipy,lowerBSHipy]=meanValue(ave_dataSHipy_s);
% 
% % 7. nship position on y direction
% [x_meanNSHipy,upperBNSHipy,lowerBNSHipy]=meanValue(ave_dataNSHipy_s);
% 
% % 8. nsankle position on y direction
% [x_meanNSAy,upperBNSAy,lowerBNSAy]=meanValue(ave_dataNSAy_s);


% save the data
 ns_time_mean = mean(time_rec)*ave_time'; % averaged time
%
 data_name = 'mean_data';
 folder_name = ['data_' model '_3D'];
%  save_outputs(Xmean,ns_time_mean,data_name,folder_name);
%%%%%%%%%%%% angles %%%%%%%%%%%%%%%%
ns_time = ns_time_mean;


% % 1. left hip angle
lhip3D = x_meanlhip';
save(['./', folder_name, '/', data_name,'_lhip3D.mat'],'lhip3D','ns_time');

% % 1. right hip angle
rhip3D = x_meanrhip';
save(['./', folder_name, '/', data_name,'_rhip3D.mat'],'rhip3D','ns_time');


OptFit3D(ave_time,upperBlhip,lowerBlhip,x_meanlhip,'lhip3D','Angle (rad)',...
    a0(1,:),2,ns_time_mean,'$\theta_{lhip}^H$','$\theta_{lhip}^f$',[-0.5 0])

OptFit3D(ave_time,upperBrhip,lowerBrhip,x_meanrhip,'rhip3D','Angle (rad)',...
    a0(2,:),2,ns_time_mean,'$\theta_{rhip}^H$','$\theta_{rhip}^f$',[-0.5 0])

% %  % 1. stance ankle angle
% % s_ankle_3D =x_meanSA';
% % save(['./', folder_name, '/',data_name,'_sankle3D.mat'],'s_ankle_3D','ns_time')
% % 
% %  % 2. stance hip angle
% % s_hip_3D = x_meanShip';
% % save(['./', folder_name, '/', data_name,'_ship3D.mat'],'s_hip_3D','ns_time');
% % 
% % % 3. non-stance hip angle
% % ns_hip_3D = x_meanNShip';
% % save(['./', folder_name, '/', data_name,'_nship3D.mat'],'ns_hip_3D','ns_time');
% % 
% % % 4. torso angle
% % torso_3D = x_meanTorso';
% % save(['./', folder_name, '/', data_name,'_torso3D.mat'],'torso_3D','ns_time');
% % 
% % % 5. hip position on z direction
% % hip_y = x_meanHipy';
% % save(['./', folder_name, '/', data_name,'_HipY.mat'],'hip_y','ns_time');

% % %% 6. ship position on y direction
% % ship_y = x_meanSHipy';
% % save(['./', folder_name, '/', data_name,'_SHipY.mat'],'ship_y','ns_time');
% % 
% % % 7. nship position on y direction
% % nship_y = x_meanNSHipy';
% % save(['./', folder_name, '/', data_name,'_NSHipY.mat'],'nship_y','ns_time');
% % 
% % % 8. nsankle position on y direction
% % nsa_y = x_meanNSAy';
% % save(['./', folder_name, '/', data_name,'_NSAY.mat'],'nsa_y','ns_time');
